package org.checkboo.hopefullyengine.collision;

import org.checkboo.hopefullyengine.boundingvolumes.AxisAlignedBoundingBox;
import org.checkboo.hopefullyengine.scenegraph.GeometryNode;
import org.checkboo.math.Vector3f;

public class Collision {

	public static boolean areColliding(GeometryNode object1, GeometryNode object2){
		float[][] planes = new float[6][4];
		AxisAlignedBoundingBox box2 = (AxisAlignedBoundingBox) object2.getBoundingVolume();
		AxisAlignedBoundingBox box1 = (AxisAlignedBoundingBox) object1.getBoundingVolume();
		
		planes[0][1] = planes[0][2] = planes[1][1] = planes[1][2] = planes[2][0] = planes[2][2] = planes[3][0] = planes[3][2] = planes[4][0] = planes[4][1] = planes[5][0] = planes[5][1] = 0.0f;
	
		planes[0][0] = box2.getV8().getX()>0 ? -1/box2.getV8().getX() : 1/box2.getV8().getX();
		planes[0][3] = box2.getV8().getX()>0 ? 1.0f : -1.0f;
		planes[1][0] = box2.getV1().getX()<0 ? -1/box2.getV1().getX() : 1/box2.getV1().getX();
		planes[1][3] = box2.getV1().getX()<0 ? 1.0f : -1.0f;
		planes[2][1] = box2.getV1().getY()>0 ? -1/box2.getV1().getY() : 1/box2.getV1().getY();
		planes[2][3] = box2.getV1().getY()>0 ? 1.0f : -1.0f;
		planes[3][1] = box2.getV8().getY()<0 ? -1/box2.getV8().getY() : 1/box2.getV8().getY();
		planes[3][3] = box2.getV8().getY()<0 ? 1.0f : -1.0f;
		planes[4][2] = box2.getV1().getZ()>0 ? -1/box2.getV1().getZ() : 1/box2.getV1().getZ();
		planes[4][3] = box2.getV1().getZ()>0 ? 1.0f : -1.0f;
		planes[5][2] = box2.getV8().getZ()<0 ? -1/box2.getV8().getZ() : 1/box2.getV8().getZ();
		planes[5][3] = box2.getV8().getZ()<0 ? 1.0f : -1.0f;

		for(int i=0;i<6;i++){
			if(planes[i][0]*box1.getV1().getX()+planes[i][1]*box1.getV1().getY()+planes[i][2]*box1.getV1().getZ()+planes[i][3] > 0)
				continue;
			if(planes[i][0]*box1.getV2().getX()+planes[i][1]*box1.getV2().getY()+planes[i][2]*box1.getV2().getZ()+planes[i][3] > 0)
				continue;
			if(planes[i][0]*box1.getV3().getX()+planes[i][1]*box1.getV3().getY()+planes[i][2]*box1.getV3().getZ()+planes[i][3] > 0)
				continue;
			if(planes[i][0]*box1.getV4().getX()+planes[i][1]*box1.getV4().getY()+planes[i][2]*box1.getV4().getZ()+planes[i][3] > 0)
				continue;
			if(planes[i][0]*box1.getV5().getX()+planes[i][1]*box1.getV5().getY()+planes[i][2]*box1.getV5().getZ()+planes[i][3] > 0)
				continue;
			if(planes[i][0]*box1.getV6().getX()+planes[i][1]*box1.getV6().getY()+planes[i][2]*box1.getV6().getZ()+planes[i][3] > 0)
				continue;
			if(planes[i][0]*box1.getV7().getX()+planes[i][1]*box1.getV7().getY()+planes[i][2]*box1.getV7().getZ()+planes[i][3] > 0)
				continue;
			if(planes[i][0]*box1.getV8().getX()+planes[i][1]*box1.getV8().getY()+planes[i][2]*box1.getV8().getZ()+planes[i][3] > 0)
				continue;
			return false;
		}
		return true;
	}
	
	public static boolean areColliding(Ray ray, GeometryNode object, float t0, float t1){
		float tmin, tmax, tymin, tymax, tzmin, tzmax;
		Vector3f min = ((AxisAlignedBoundingBox)object.getBoundingVolume()).getV7();
		Vector3f max = ((AxisAlignedBoundingBox)object.getBoundingVolume()).getV2();
		
		if(ray.getDirection().getX() >= 0){
			tmin = (min.getX() - ray.getOrigin().getX()) / ray.getDirection().getX();
			tmax = (max.getX() - ray.getOrigin().getX()) / ray.getDirection().getX();
		}
		else{
			tmin = (max.getX() - ray.getOrigin().getX()) / ray.getDirection().getX();
			tmax = (min.getX() - ray.getOrigin().getX()) / ray.getDirection().getX();
		}
		
		if(ray.getDirection().getY() >= 0){
			tymin = (min.getY() - ray.getOrigin().getY()) / ray.getDirection().getY();
			tymax = (max.getY() - ray.getOrigin().getY()) / ray.getDirection().getY();
		}
		else{
			tymin = (max.getY() - ray.getOrigin().getY()) / ray.getDirection().getY();
			tymax = (min.getY() - ray.getOrigin().getY()) / ray.getDirection().getY();
		}
		
		if((tmin > tymax) || (tymin > tmax)){
			return false;
		}
		
		if(tymin > tmin)
			tmin = tymin;
		if(tymax < tmax)
			tmax = tymax;
		
		if(ray.getDirection().getZ() >= 0){
			tzmin = (min.getZ() - ray.getOrigin().getZ()) / ray.getDirection().getZ();
			tzmax = (max.getZ() - ray.getOrigin().getZ()) / ray.getDirection().getZ();
		}
		else{
			tzmin = (max.getZ() - ray.getOrigin().getZ()) / ray.getDirection().getZ();
			tzmax = (min.getZ() - ray.getOrigin().getZ()) / ray.getDirection().getZ();
		}
		
		if((tmin > tzmax || tzmin > tmax)){
			return false;
		}
		if(tzmin > tmin)
			tmin = tzmin;
		if(tzmax < tmax)
			tmax = tzmax;
		
		return ((tmin < t1) && (tmax > t0));
	}
}
